#include "motor42.h"

/*
*  PA1: Direction
*  PA2: Enable
*  PA3: Pulse
*
*  1 circle = 6400 pulse
*  1 puluse == 360/6400 deg == 0.05625 deg
*  
*/

#define ANTI_CLOCKWISE 0
#define CLOCKWISE 1

#define MAX_DTHETA 1.0f


int32_t Clockwise_Cnt=0,Anti_Clockwise_Cnt=0;
float Motor_Location=0,Expect_Location=0;
extern float theta;

void Motor_Init(void)
{
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, GPIO_PIN_RESET);
}


void Motor_Send_One_Pulse(uint8_t direction)
{
  if(direction==ANTI_CLOCKWISE){HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1,GPIO_PIN_SET);Anti_Clockwise_Cnt++;}
  else if(direction==CLOCKWISE){HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1,GPIO_PIN_RESET);Clockwise_Cnt++;}
  HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3,GPIO_PIN_SET);
  delay_us(10);
  HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3,GPIO_PIN_RESET);
	delay_us(40);
}

void Update_Motor_Status(void)
{
  Motor_Location = (Anti_Clockwise_Cnt - Clockwise_Cnt) * 0.05625f + 180.0f;
  if(Motor_Location > 360){Motor_Location=0;Anti_Clockwise_Cnt=0;Clockwise_Cnt=0;}
}

void Update_Expect_Location(void)
{
	Expect_Location = theta;
}

// tool function
float abs_f(float num)
{
  if(num<0)return -num;
  else return num;
}

void Update_Motor_Location(void)
{
	float delta_theta=0;
	uint8_t dir=0;
	delta_theta = theta - Motor_Location;
	if(delta_theta>0)dir=0;
	else dir=1;
	if(delta_theta>MAX_DTHETA)delta_theta=MAX_DTHETA;
	else if(delta_theta<-MAX_DTHETA)delta_theta=-MAX_DTHETA;
	for(uint8_t i=0;i<(uint8_t)(abs_f(delta_theta)/0.05625f);i++)
	{
		Motor_Send_One_Pulse(dir);
	}
}





